#include "Sensor.h"
#include "InterpolateLinear.h"

Sensor::Sensor (const QString &name, const QString &intrinsicFile,
		const QString &extrinsicFile) : _name (name)
{
	if (extrinsicFile != "")
		_readExtrinsic (extrinsicFile);
	__intrinsic.readFrom (intrinsicFile);
	_depthImage = QImage ((unsigned int)__intrinsic.fw (),
			(unsigned int)__intrinsic.fh (), QImage::Format_RGB16);
	_depthImage.fill (Qt::black);
	_rgbImage = QImage ((unsigned int)__intrinsic.fw (),
			(unsigned int)__intrinsic.fh (), QImage::Format_RGB32);
	_rgbImage.fill (Qt::black);
	_playerImage = QImage ((unsigned int)__intrinsic.fw (),
		(unsigned int)__intrinsic.fh (), QImage::Format_Indexed8);
	_playerImage.fill (Qt::black);
}

Sensor::~Sensor ()
{
}

const QImage& Sensor::getDepthImage () const
{
	return _depthImage;
}
QImage& Sensor::getDepthImage ()
{
	return _depthImage;
}

const QImage& Sensor::getRGBImage () const
{
	return _rgbImage;
}
QImage& Sensor::getRGBImage ()
{
	return _rgbImage;
}

const SensorParameter& Sensor::getSensorParameter () const
{
	return __sensorParameter;
}
SensorParameter& Sensor::getSensorParameter ()
{
	return __sensorParameter;
}

void Sensor::showData (const QString& titlePrefix)
{
// 	if (_depthImage) {
// 		cvShowImage ((titlePrefix + "Depth Image - " + _name).c_str (),
// 				_depthImage);
// 	}
// 	if (_rgbImage) {
// 		cvShowImage ((titlePrefix + "RGB Image - " + _name).c_str (),
// 				_rgbImage);
// 	}
}

const glm::mat4x4& Sensor::getExtrinsicMat () const
{
	return __extrinsicMat;
}

void Sensor::setExtrinsicMat (const glm::mat4x4& extrinsic)
{
	__extrinsicMat = extrinsic;
	__inverseExtrinsicMat = glm::inverse (__extrinsicMat);
}

void Sensor::setExtrinsicMat (const QString& extrinsicFile)
{
	_readExtrinsic (extrinsicFile);
}

glm::mat4x4& Sensor::getToWorldMat ()
{
	return __inverseExtrinsicMat;
}

const glm::mat4x4& Sensor::getToWorldMat () const
{
	return __inverseExtrinsicMat;
}

QString Sensor::getName () const
{
	return _name;
}

glm::vec3 Sensor::get3DPoint (float x, float y) const
{
	return (__inverseExtrinsicMat * glm::vec4 (get3DPointInCam (x, y), 1.0)).
			swizzle (glm::X, glm::Y, glm::Z);
}

glm::vec3 Sensor::get3DPointInCam (float x, float y) const
{
	QRgb depth = InterpolateLinear ().interpolate (_depthImage, QPointF (x, y));
	return unprojectToCam (glm::vec3 (x, y, (float)depth));
}

glm::vec3 Sensor::unprojectToCam (glm::vec3 pi) const
{
	return __intrinsic.unproject (pi);
}

glm::vec3 Sensor::unprojectToWorld (glm::vec3 pi) const
{
	return (__inverseExtrinsicMat * glm::vec4 (unprojectToCam (pi), 1.0)).
			swizzle (glm::X, glm::Y, glm::Z);
}

void Sensor::_undistortImage (QImage& src, QImage& dst) const
{
	const cv::Matx33f& cameraMatrix = __intrinsic.matrixCvMat ();
	const cv::Vec< float, 5 >& distCoeffs = __intrinsic.distCoeffsCvMat ();
 	cv::Mat srcCvImg (src.height (), src.width (),
 			qImageFormat2CvImageType (src.format ()), (void*)src.constBits ());
 	cv::Mat dstCvImg (dst.height (), dst.width (),
 			qImageFormat2CvImageType (dst.format ()), (void*)dst.bits ());
// 	srcCvImg.copyTo (dstCvImg);
	
// 	memcpy (dst.bits (), src.bits (), dst.byteCount ());
	cv::undistort (srcCvImg, dstCvImg, cameraMatrix, distCoeffs);
// 	imshow ("123", dstCvImg);
//	dst = src;
}

void Sensor::__handleNewData ()
{
	if (!__depthMetaData.isNull ()) {
		if (__depthMetaData.size () != _depthImage.size ())
			_preprocessTempDepth = __depthMetaData.scaled (_depthImage.size ());
		else
			_preprocessTempDepth = __depthMetaData;
		_undistortImage (_preprocessTempDepth, _depthImage);
	}
	if (!__playerMetaData.isNull ()) {
		if (__playerMetaData.size () != _playerImage.size ())
			_preprocessTempPlayer = __playerMetaData.scaled (_playerImage.size ());
		else
			_preprocessTempPlayer = __playerMetaData;
		_undistortImage (_preprocessTempPlayer, _playerImage);
	}
	if (!__rgbMetaData.isNull ()) {
		if (__rgbMetaData.size () != _rgbImage.size ())
			_preprocessTempRgb = __rgbMetaData.scaled (_rgbImage.size ());
		else
			_preprocessTempRgb = __rgbMetaData;
		_undistortImage (_preprocessTempRgb, _rgbImage);
	}
	emit newFrameReady ();
}

const SensorIntrinsic& Sensor::getIntrinsic () const
{
	return __intrinsic;
}

void Sensor::setIntrinsic (const SensorIntrinsic& intrinsic)
{
	__intrinsic = intrinsic;
}

void Sensor::setIntrinsic (const QString& intrinsicFile)
{
	__intrinsic.readFrom (intrinsicFile);
}

void Sensor::_readExtrinsic (const QString& file)
{
	std::ifstream fIn (file.toStdString ());
	for (int c = 0; c < 4; ++c)
		for (int r = 0; r < 4; ++r)
			fIn >> __extrinsicMat[c][r];
	if (!fIn)
		throw std::exception ("Sensor::_readExtrinsic(): "
				"Fail to read extrinsic file");
	__inverseExtrinsicMat = glm::inverse (__extrinsicMat);
}

void Sensor::_readInvProjMat (const QString &file)
{
	std::ifstream fIn (file.toStdString ());
	for (int r = 0; r < 3; ++r)
		for (int c = 0; c < 4; ++c)
			fIn >> __invProjMat[c][r];
	if (!fIn)
		throw std::exception ("Sensor::_readExtrinsic(): "
		"Fail to read extrinsic file");
	__invProjMat /= glm::length (glm::vec3 (
			__invProjMat[0][2], __invProjMat[1][2], __invProjMat[2][2]));
	__invProjMat[0][3] = __invProjMat[1][3] = __invProjMat[2][3] = 0.0f;
	__invProjMat[3][3] = 1.0f;
	__invProjMat = glm::inverse (__invProjMat);
}

void Sensor::setInvProjMat (const QString& projFile)
{
	_readInvProjMat (projFile);
}

const glm::mat4x4 & Sensor::getInvProjMat () const
{
	return __invProjMat;
}

QImage& Sensor::getPlayerImage ()
{
	return _playerImage;
}

const QImage& Sensor::getPlayerImage () const
{
	return _playerImage;
}
